/*
 * This file is part of the Sx Framework Library.
 * 
 * Copyright (C) 2013 University of Colorado Denver
 * <min.choi@ucdenver.edu> <shane.transue@ucdenver.edu>
 * 
 * Permission is hereby granted, free of charge, to any person obtaining a copy 
 * of this software and associated documentation files (the "Software"), to deal
 * in the Software without restriction, including without limitation the rights
 * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 
 * copies of the Software, and to permit persons to whom the Software is 
 * furnished to do so, subject to the following conditions:
 * 
 * The above copyright notice and this permission notice shall be included in 
 * all copies or substantial portions of the Software.
 * 
 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 
 * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 
 * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 
 * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 
 * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING 
 * FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER 
 * DEALINGS IN THE SOFTWARE.
 */
#include <sxIntersections.h>

bool Sx::Math::Intersections::SegmentIntersectTrianglef(const Eigen::Vector3f& segment_p0, const Eigen::Vector3f& segmnet_p1, const Eigen::Vector3f& triangle_p0, const Eigen::Vector3f& triangle_p1, const Eigen::Vector3f& triangle_p2, Eigen::Vector3f& point) {
	Eigen::Vector3f u, v, n;
	Eigen::Vector3f dir, w0, w;
	long double r, a, b;

	u = triangle_p1 - triangle_p0;
	v = triangle_p2 - triangle_p0;
	n = u.cross(v);

	if ( n == Eigen::Vector3f::Zero() )
		return false;

	dir = segmnet_p1 - segment_p0;
	w0 = segment_p0 - triangle_p0;
	a = -n.dot(w0);
	b = n.dot(dir);
	if ( std::abs(b) < std::numeric_limits<float>::epsilon() ) {
		if ( a == 0 ) {
			point = Eigen::Vector3f(point.x(), point.y(), point.z());
			return true;
		}
		else return false;
	}

	r = a / b;
	if ( r < 0.0f || r > 1.0f ) return false;

	Eigen::Vector3f temp = dir * static_cast<float>(r);
	Eigen::Vector3f intersection = segment_p0 + temp;
	point = intersection;

	double uu, uv, vv, wu, wv, D;
	uu = static_cast<long double>(u.dot(u));
	uv = static_cast<long double>(u.dot(v));
	vv = static_cast<long double>(v.dot(v));
	w = intersection - triangle_p0;
	wu = static_cast<long double>(w.dot(u));
	wv = static_cast<long double>(w.dot(v));
	D = uv * uv - uu * vv;

	long double s, t;
	s = (uv * wv - vv * wu) / D;
	if ( s < 0.0f || s > 1.0f ) return false;
	t = (uv * wu - uu * wv) / D;
	if ( t < 0.0f || (s + t) > 1.0f ) return false;

	point = Eigen::Vector3f(point.x(), point.y(), point.z());
	return true;
}

bool Sx::Math::Intersections::SegmentIntersectTriangled(const Eigen::Vector3d& segment_p0, const Eigen::Vector3d& segmnet_p1, const Eigen::Vector3d& triangle_p0, const Eigen::Vector3d& triangle_p1, const Eigen::Vector3d& triangle_p2, Eigen::Vector3d& point) {
	Eigen::Vector3d u, v, n;
	Eigen::Vector3d dir, w0, w;
	double r, a, b;

	u = triangle_p1 - triangle_p0;
	v = triangle_p2 - triangle_p0;
	n = u.cross(v);

	if ( n == Eigen::Vector3d::Zero() )
		return false;

	dir = segmnet_p1 - segment_p0;
	w0 = segment_p0 - triangle_p0;
	a = -n.dot(w0);
	b = n.dot(dir);
	if ( std::abs(b) < 0.0001 ) {
		if ( a == 0 ) {
			point = Eigen::Vector3d(point.x(), point.y(), point.z());
			return true;
		}
		else return false;
	}

	r = a / b;
	if ( r < 0.0 || r > 1.0 ) return false;

	Eigen::Vector3d temp = dir * r;
	Eigen::Vector3d intersection = segment_p0 + temp;
	point = intersection;

	double uu, uv, vv, wu, wv, D;
	uu = u.dot(u);
	uv = u.dot(v);
	vv = v.dot(v);
	w = intersection - triangle_p0;
	wu = w.dot(u);
	wv = w.dot(v);
	D = uv * uv - uu * vv;

	double s, t;
	s = (uv * wv - vv * wu) / D;
	if ( s < 0.0 || s > 1.0 ) return false;
	t = (uv * wu - uu * wv) / D;
	if ( t < 0.0 || (s + t) > 1.0 ) return false;

	point = Eigen::Vector3d(point.x(), point.y(), point.z());
	return true;
}

/*
 * AABB Intersection Functions (Cohen-Sutherland Clipping Outcodes)
 * These constants are defined for the calc_outcode() function as part of the 
 * SegmentIntersectAxisAlignedBoundingBox() implementation.
 */
#define CLIP_RIGHT	(1<<0)
#define CLIP_LEFT	(1<<1)
#define CLIP_TOP	(1<<2)
#define CLIP_BOTTOM	(1<<3)
#define CLIP_FRONT	(1<<4)
#define CLIP_BACK	(1<<5)

/*
 * This function is a utility function for the SegmentIntersectAxisAlignedBoundingBox()
 * algorithm.
 */
static unsigned long calc_outcode(const Eigen::Vector3f& bbox_min, const Eigen::Vector3f& bbox_max, const Eigen::Vector3f& pnt) {
	unsigned long outcode = 0;

	if( pnt.x() > bbox_max.x() ) {
		outcode |= CLIP_RIGHT;
	} else if( pnt.x() < bbox_min.x() ) {
		outcode |= CLIP_LEFT;
	}
	if( pnt.y() > bbox_max.y() ) {
		outcode |= CLIP_TOP;
	} else if( pnt.y() < bbox_min.y() ) {
		outcode |= CLIP_BOTTOM;
	}
	if( pnt.z() > bbox_max.z() ) {
		outcode |= CLIP_BACK;
	} else if( pnt.z() < bbox_min.z() ) {
		outcode |= CLIP_FRONT;
	}

	return outcode;
}

bool Sx::Math::Intersections::SegmentIntersectAxisAlignedBoundingBox(const Eigen::Vector3f& min, const Eigen::Vector3f& max, const Eigen::Vector3f& segment_p0, const Eigen::Vector3f& segment_p1, Eigen::Vector3f& point) {
	unsigned long outcode1, outcode2;

	outcode1 = calc_outcode(min, max, segment_p0);
	if( outcode1 == 0 ) {
		point = segment_p0;
		return true;
	}

	outcode2 = calc_outcode(min, max, segment_p1);
	if( outcode2 == 0 ) {
		point = segment_p1;
		return true;
	}

	if( (outcode1 & outcode2) > 0 ) {
		return false;
	}

	if( outcode1 & (CLIP_RIGHT | CLIP_LEFT) ) {
		if( outcode1 & CLIP_RIGHT ) {
			point.x() = max.x();
		} else {
			point.x() = min.x();
		}
		float x1 = segment_p1.x() - segment_p0.x();
		float x2 = point.x() - segment_p0.x();
		point.y() = segment_p0.y() + x2 * (segment_p1.y() - segment_p0.y()) / x1;
		point.z() = segment_p0.z() + x2 * (segment_p1.z() - segment_p0.z()) / x1;

		if( point.y() <= max.y() && point.y() >= min.y() && point.z() <= max.z() && point.z() >= min.z() ) {
			return true;
		}
	}

	if( outcode1 & (CLIP_TOP | CLIP_BOTTOM) ) {
		if( outcode1 & CLIP_TOP ) {
			point.y() = max.y();
		} else {
			point.y() = min.y();
		}
		float y1 = segment_p1.y() - segment_p0.y();
		float y2 = point.y() - segment_p0.y();
		point.x() = segment_p0.x() + y2 * (segment_p1.x() - segment_p0.x()) / y1;
		point.z() = segment_p0.z() + y2 * (segment_p1.z() - segment_p0.z()) / y1;

		if( point.x() <= max.x() && point.x() >= min.x() && point.z() <= max.z() && point.z() >= min.z() ) {
			return true;
		}
	}

	if( outcode1 & (CLIP_FRONT | CLIP_BACK) ) {
		if( outcode1 & CLIP_BACK ) {
			point.z() = max.z();
		} else {
			point.z() = min.z();
		}
		float z1 = segment_p1.z() - segment_p0.z();
		float z2 = point.z() - segment_p0.z();
		point.x() = segment_p0.x() + z2 * (segment_p1.x() - segment_p0.x()) / z1;
		point.y() = segment_p0.y() + z2 * (segment_p1.y() - segment_p0.y()) / z1;

		if( point.x() <= max.x() && point.x() >= min.x() && point.y() <= max.y() && point.y() >= min.y() ) {
			return true;
		}
	}

	return false;
}

/*
 * Real Time Collision Detection pg. 178
 */
bool Sx::Math::Intersections::SegmentIntersectSphere(const Eigen::Vector3f& segment_p0, const Eigen::Vector3f& segment_p1, const Eigen::Vector3f& center, float radius, Eigen::Vector3f& point) {
	Eigen::Vector3f p = segment_p0;
	long double mag = (segment_p1 - segment_p0).norm();
	Eigen::Vector3f d = ((segment_p1 - segment_p0) / static_cast<float>(mag)).normalized();

	Eigen::Vector3f m = p - center;
	long double b = m.dot(d);
	long double c = ((long double)m.x() * (long double)m.x() + (long double)m.y() * (long double)m.y() + (long double)m.z() * (long double)m.z()) - (long double)1;

	if ( c > 0.0f && b > 0.0f ) return false;
	long double discr = b*b - c;

	if ( discr < 0.0f ) return false;
	long double t = -b - std::sqrt(discr);

	if ( t >= mag ) return false;
	if ( t < 0.0f ) t = 0.0f;

	point = p + static_cast<float>(t) * d;
	return true;
}

bool Sx::Math::Intersections::PointInPlane(const Eigen::Vector3d& planeNormal, const Eigen::Vector3d& point, bool& onSameSideAsNormal) {
	return PointInPlane(planeNormal, point, onSameSideAsNormal, 0.0);
}

bool Sx::Math::Intersections::PointInPlane(const Eigen::Vector3d& planeNormal, const Eigen::Vector3d& point, bool& onSameSideAsNormal, float threshold) {
	Eigen::Vector3d nPlaneNormal = planeNormal.normalized();
	double res = point.dot(nPlaneNormal);
	if ( res >= -std::abs(threshold) && res <= std::abs(threshold) ) return true;
	if ( res > 0 ) onSameSideAsNormal = true;
	if ( res < 0 ) onSameSideAsNormal = false;
	return false;
}
